import rclpy
import cv2
import cv_bridge
from rclpy.node import Node
from sensor_msgs.msg import Image, CompressedImage

class ImageToCompressedImageNode(Node):
    def __init__(self):
        super().__init__('image_to_compressed_image')
        self.get_logger().info("Starting image to compressed image node")
        self.image_sub = self.create_subscription(Image, 'image', self.image_callback, 10)
        self.pub = self.create_publisher(CompressedImage, '/image/compressed', 10)
    
    def image_callback(self, msg):
        self.get_logger().info("Processing image callback")
        try:
            # Create CompressedImage message
            compressed_image_msg = CompressedImage()
            compressed_image_msg.header = msg.header
            compressed_image_msg.format = "jpeg"
            compressed_image_msg.data = msg.data  # Directly copy the data from Image message
            # Publish the compressed image
            self.pub.publish(compressed_image_msg)
        except Exception as e:
            self.get_logger().error(f"Error: {e}")

def main(args=None):
    rclpy.init(args=args)
    node = ImageToCompressedImageNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
